#include "config.h"
#include "utilities.h"
#include "eigen.h"

int eigenQREV (fit_double Q[], fit_double pi[], int n, 
               fit_double Root[], fit_double U[], fit_double V[], fit_double spacesqrtpi[])
{
/* 
   This finds the eigen solution of the rate matrix Q for a time-reversible 
   Markov process, using the algorithm for a real symmetric matrix.
   Rate matrix Q = S * diag{pi} = U * diag{Root} * V, 
   where S is symmetrical, all elements of pi are positive, and U*V = I.
   space[n] is for storing (fit_double)sqrt(pi).

   [U 0] [Q_0 0] [U^-1 0]    [Root  0]
   [0 I] [0   0] [0    I]  = [0     0]

   Ziheng Yang, 25 December 2001 (ref is CME/eigenQ.pdf)
*/
   int i,j, inew, jnew, nnew, status;
   fit_double *pi_sqrt=spacesqrtpi, small=1e-100;

   for(j=0,nnew=0; j<n; j++)
      if(pi[j]>small)
         pi_sqrt[nnew++]=(fit_double)sqrt(pi[j]);

   /* store in U the symmetrical matrix S = (fit_double)sqrt(D) * Q * (fit_double)sqrt(-D) */

   if(nnew==n) {
      for(i=0; i<n; i++)
         for(j=0,U[i*n+i] = Q[i*n+i]; j<i; j++)
            U[i*n+j] = U[j*n+i] = (Q[i*n+j] * pi_sqrt[i]/pi_sqrt[j]);
      status=eigenRealSym(U, n, Root, V);
      for(i=0;i<n;i++) for(j=0;j<n;j++)  V[i*n+j] = U[j*n+i] * pi_sqrt[j];
      for(i=0;i<n;i++) for(j=0;j<n;j++)  U[i*n+j] /= pi_sqrt[i];
   }
   else {
      for(i=0,inew=0; i<n; i++) {
         if(pi[i]>small) {
            for(j=0,jnew=0; j<i; j++) 
               if(pi[j]>small) {
                  U[inew*nnew+jnew] = U[jnew*nnew+inew] 
                                    = Q[i*n+j] * pi_sqrt[inew]/pi_sqrt[jnew];
                  jnew++;
               }
            U[inew*nnew+inew] = Q[i*n+i];
            inew++;
         }
      }

      status=eigenRealSym(U, nnew, Root, V);

      for(i=n-1,inew=nnew-1; i>=0; i--)   /* construct Root */
         Root[i] = (pi[i]>small ? Root[inew--] : 0);
      for(i=n-1,inew=nnew-1; i>=0; i--) {  /* construct V */
         if(pi[i]>small) {
            for(j=n-1,jnew=nnew-1; j>=0; j--)
               if(pi[j]>small) {
                  V[i*n+j] = U[jnew*nnew+inew]*pi_sqrt[jnew];
                  jnew--;
               }
               else 
                  V[i*n+j] = (i==j);
            inew--;
         }
         else 
            for(j=0; j<n; j++)  V[i*n+j] = (i==j);
      }
      for(i=n-1,inew=nnew-1; i>=0; i--) {  /* construct U */
         if(pi[i]>small) {
            for(j=n-1,jnew=nnew-1;j>=0;j--)
               if(pi[j]>small) {
                  U[i*n+j] = U[inew*nnew+jnew]/pi_sqrt[inew];
                  jnew--;
               }
               else 
                  U[i*n+j] = (i==j);
            inew--;
         }
         else 
            for(j=0;j<n;j++)
               U[i*n+j] = (i==j);
      }
   }

/*   This routine is also used for P(t) as well as Q. */

   if((fit_double)fabs(Root[0])>1e-10) printf("Root[0] = %.5e\n",Root[0]);
   Root[0]=0; 
   return(status);
}


/* eigen solution for real symmetric matrix */


int eigenRealSym(fit_double A[], int n, fit_double Root[], fit_double work[])
{
/* This finds the eigen solution of a real symmetrical matrix A[n*n].  In return, 
   A has the right vectors and Root has the eigenvalues. work[n] is the working space.
   The matrix is first reduced to a tridiagonal matrix using HouseholderRealSym(), 
   and then using the QL algorithm with implicit shifts.  

   Adapted from routine tqli in Numerical Recipes in C, with reference to LAPACK
   Ziheng Yang, 23 May 2001
*/
   int status=0;
   HouseholderRealSym(A, n, Root, work);
   status=EigenTridagQLImplicit(Root, work, n, A);
   EigenSort(Root, A, n);

   return(status);
}


void EigenSort(fit_double d[], fit_double U[], int n)
{
/* this sorts the eigen values d[] and rearrange the (right) eigen vectors U[]
*/
   int k,j,i;
   fit_double p;

   for (i=0;i<n-1;i++) {
      p=d[k=i];
      for (j=i+1;j<n;j++)
         if (d[j] >= p) p=d[k=j];
      if (k != i) {
         d[k]=d[i];
         d[i]=p;
         for (j=0;j<n;j++) {
            p=U[j*n+i];
            U[j*n+i]=U[j*n+k];
            U[j*n+k]=p;
         }
      }
   }
}

void HouseholderRealSym(fit_double a[], int n, fit_double d[], fit_double e[])
{
/* This uses HouseholderRealSym transformation to reduce a real symmetrical matrix 
   a[n*n] into a tridiagonal matrix represented by d and e.
   d[] is the diagonal (eigends), and e[] the off-diagonal.
*/
   int m,k,j,i;
   fit_double scale,hh,h,g,f;

   for (i=n-1;i>=1;i--) {
      m=i-1;
      h=scale=0;
      if (m > 0) {
         for (k=0;k<=m;k++)
            scale += (fit_double)fabs(a[i*n+k]);
         if (scale == 0)
            e[i]=a[i*n+m];
         else {
            for (k=0;k<=m;k++) {
               a[i*n+k] /= scale;
               h += a[i*n+k]*a[i*n+k];
            }
            f=a[i*n+m];
            g=(f >= 0 ? -(fit_double)sqrt(h) : (fit_double)sqrt(h));
            e[i]=scale*g;
            h -= f*g;
            a[i*n+m]=f-g;
            f=0;
            for (j=0;j<=m;j++) {
               a[j*n+i]=a[i*n+j]/h;
               g=0;
               for (k=0;k<=j;k++)
                  g += a[j*n+k]*a[i*n+k];
               for (k=j+1;k<=m;k++)
                  g += a[k*n+j]*a[i*n+k];
               e[j]=g/h;
               f += e[j]*a[i*n+j];
            }
            hh=f/(h*2);
            for (j=0;j<=m;j++) {
               f=a[i*n+j];
               e[j]=g=e[j]-hh*f;
               for (k=0;k<=j;k++)
                  a[j*n+k] -= (f*e[k]+g*a[i*n+k]);
            }
         }
      } 
      else
         e[i]=a[i*n+m];
      d[i]=h;
   }
   d[0]=e[0]=0;

   /* Get eigenvectors */
   for (i=0;i<n;i++) {
      m=i-1;
      if (d[i]) {
         for (j=0;j<=m;j++) {
            g=0;
            for (k=0;k<=m;k++)
               g += a[i*n+k]*a[k*n+j];
            for (k=0;k<=m;k++)
               a[k*n+j] -= g*a[k*n+i];
         }
      }
      d[i]=a[i*n+i];
      a[i*n+i]=1;
      for (j=0;j<=m;j++) a[j*n+i]=a[i*n+j]=0;
   }
}


int EigenTridagQLImplicit(fit_double d[], fit_double e[], int n, fit_double z[])
{
/* This finds the eigen solution of a tridiagonal matrix represented by d and e.  
   d[] is the diagonal (eigenvalues), e[] is the off-diagonal
   z[n*n]: as input should have the identity matrix to get the eigen solution of the 
   tridiagonal matrix, or the output from HouseholderRealSym() to get the 
   eigen solution to the original real symmetric matrix.
   z[n*n]: has the orthogonal matrix as output

   Adapted from routine tqli in Numerical Recipes in C, with reference to
   LAPACK fortran code.
   Ziheng Yang, May 2001
*/
   int m,j,iter,niter=30, status=0, i,k;
   fit_double s,r,p,g,f,dd,c,b, aa,bb;

   for (i=1;i<n;i++) e[i-1]=e[i];  e[n-1]=0;
   for (j=0;j<n;j++) {
      iter=0;
      do {
         for (m=j;m<n-1;m++) {
            dd=(fit_double)fabs(d[m])+(fit_double)fabs(d[m+1]);
            if ((fit_double)fabs(e[m])+dd == dd) break;  /* ??? */
         }
         if (m != j) {
            if (iter++ == niter) {
               status=-1;
               break;
            }
            g=(d[j+1]-d[j])/(2*e[j]);

            /* r=pythag(g,1); */

            if((aa=(fit_double)fabs(g))>1)  r=aa*(fit_double)sqrt(1+1/(g*g));
            else                r=(fit_double)sqrt(1+g*g);

            g=d[m]-d[j]+e[j]/(g+SIGN(r,g));
            s=c=1;
            p=0;
            for (i=m-1;i>=j;i--) {
               f=s*e[i];
               b=c*e[i];

               /*  r=pythag(f,g);  */
               aa=(fit_double)fabs(f); bb=(fit_double)fabs(g);
               if(aa>bb)       { bb/=aa;  r=aa*(fit_double)sqrt(1+bb*bb); }
               else if(bb==0)             r=0;
               else            { aa/=bb;  r=bb*(fit_double)sqrt(1+aa*aa); }

               e[i+1]=r;
               if (r == 0) {
                  d[i+1] -= p;
                  e[m]=0;
                  break;
               }
               s=f/r;
               c=g/r;
               g=d[i+1]-p;
               r=(d[i]-g)*s+2*c*b;
               d[i+1]=g+(p=s*r);
               g=c*r-b;
               for (k=0;k<n;k++) {
                  f=z[k*n+i+1];
                  z[k*n+i+1]=s*z[k*n+i]+c*f;
                  z[k*n+i]=c*z[k*n+i]-s*f;
               }
            }
            if (r == 0 && i >= j) continue;
            d[j]-=p; e[j]=g; e[m]=0;
         }
      } while (m != j);
   }
   return(status);
}


/***********************************************************
*  This eigen() routine works for eigenvalue/vector analysis
*         for real general square matrix A
*         A will be destroyed
*         rr,ri are vectors containing eigenvalues
*         vr,vi are matrices containing (right) eigenvectors
*
*              A*[vr+vi*i] = [vr+vi*i] * diag{rr+ri*i}
*
*  Algorithm: Handbook for Automatic Computation, vol 2
*             by Wilkinson and Reinsch, 1971
*             most of source codes were taken from a public domain
*             solftware called MATCALC.
*  Credits:   to the authors of MATCALC
*
*  return     -1 not converged
*              0 no complex eigenvalues/vectors
*              1 complex eigenvalues/vectors
*  Tianlin Wang at University of Illinois
*  Thu May  6 15:22:31 CDT 1993
***************************************************************/


#define BASE        2    /* base of floating point arithmetic */
#define DIGITS     40    /* no. of digits to the base BASE in the fraction */
/*
#define DIGITS     53
*/
#define MAXITER    30    /* max2. no. of iterations to converge */

#define pos(i,j,n)      ((i)*(n)+(j))


/* rr/vr : real parts of eigen values/vectors */
/* ri/vi : imaginary part s of eigen values/vectors */


int eigen(int job, fit_double *A, int n, fit_double *rr, fit_double *ri,
          fit_double *vr, fit_double *vi, fit_double *work)
{
/* job=0: eigen values only
       1: both eigen values and eigen vectors
   fit_double w[n*2]: work space
*/
    int low,hi,i,j,k, it, istate=0;
    fit_double tiny=(fit_double)sqrt((fit_double)pow((fit_double)BASE,(fit_double)(1-DIGITS))), t;

/*     printf("EIGEN\n"); */

    balance(A,n,&low,&hi,work);
    elemhess(job,A,n,low,hi,vr,vi, (int*)(work+n));
    if (-1 == realeig(job,A,n,low,hi,rr,ri,vr,vi)) return (-1);
    if (job) unbalance(n,vr,vi,low,hi,work);

/* sort, added by Z. Yang */
   for (i=0; i<n; i++) {
       for (j=i+1,it=i,t=rr[i]; j<n; j++)
           if (t<rr[j]) { t=rr[j]; it=j; }
       rr[it]=rr[i];   rr[i]=t;
       t=ri[it];       ri[it]=ri[i];  ri[i]=t;
       for (k=0; k<n; k++) {
          t=vr[k*n+it];  vr[k*n+it]=vr[k*n+i];  vr[k*n+i]=t;
          t=vi[k*n+it];  vi[k*n+it]=vi[k*n+i];  vi[k*n+i]=t;
       }
       if ((fit_double)fabs(ri[i])>tiny) istate=1;
   }

    return (istate) ;
}

/* complex funcctions
*/

complex compl (fit_double re,fit_double im)
{
    complex r;

    r.re = re;
    r.im = im;
    return(r);
}

complex _conj (complex a)
{
    a.im = -a.im;
    return(a);
}

#define csize(a) ((fit_double)fabs(a.re)+(fit_double)fabs(a.im))

complex cplus (complex a, complex b)
{
   complex c;
   c.re = a.re+b.re;
   c.im = a.im+b.im;
   return (c);
}

complex cminus (complex a, complex b)
{
   complex c;
   c.re = a.re-b.re;
   c.im = a.im-b.im;
   return (c);
}

complex cby (complex a, complex b)
{
   complex c;
   c.re = a.re*b.re-a.im*b.im ;
   c.im = a.re*b.im+a.im*b.re ;
   return (c);
}

complex cdiv (complex a,complex b)
{
    fit_double ratio, den;
    complex c;

    if ((fit_double)fabs(b.re) <= (fit_double)fabs(b.im)) {
        ratio = b.re / b.im;
        den = b.im * (1 + ratio * ratio);
        c.re = (a.re * ratio + a.im) / den;
        c.im = (a.im * ratio - a.re) / den;
    }
    else {
        ratio = b.im / b.re;
        den = b.re * (1 + ratio * ratio);
        c.re = (a.re + a.im * ratio) / den;
        c.im = (a.im - a.re * ratio) / den;
    }
    return(c);
}

complex my_cexp (complex a)
{
   complex c;
   c.re = (fit_double)exp(a.re);
   if ((fit_double)fabs(a.im)==0) c.im = 0;
   else  { c.im = c.re*(fit_double)sin(a.im); c.re*=(fit_double)cos(a.im); }
   return (c);
}

complex cfactor (complex x, fit_double a)
{
   complex c;
   c.re = a*x.re;
   c.im = a*x.im;
   return (c);
}

int cxtoy (complex *x, complex *y, int n)
{
   int i;
   For (i,n) y[i]=x[i];
   return (0);
}

int cmatby (complex *a, complex *b, complex *c, int n,int m,int k)
/* a[n*m], b[m*k], c[n*k]  ......  c = a*b
*/
{
   int i,j,i1;
   complex t;

   For (i,n)  For(j,k) {
       for (i1=0,t=compl(0,0); i1<m; i1++)
           t = cplus (t, cby(a[i*m+i1],b[i1*k+j]));
       c[i*k+j] = t;
   }
   return (0);
}

int cmatinv( complex *x, int n, int m, fit_double *space)
{
/* x[n*m]  ... m>=n
*/
   int i,j,k, *irow=(int*) space;
   fit_double xmaxsize, ee=1e-20;
   complex t,t1;

   For(i,n)  {
       xmaxsize = 0.;
       for (j=i; j<n; j++) {
          if ( xmaxsize < csize (x[j*m+i]))  {
               xmaxsize = csize (x[j*m+i]);
               irow[i] = j;
          }
       }
       if (xmaxsize < ee)   {
           printf("\nDet goes to zero at %8d!\t\n", i+1);
           return(-1);
       }
       if (irow[i] != i) {
           For(j,m) {
                t = x[i*m+j];
                x[i*m+j] = x[irow[i]*m+j];
                x[ irow[i]*m+j] = t;
           }
       }
       t = cdiv (compl(1,0), x[i*m+i]);
       For(j,n) {
           if (j == i) continue;
           t1 = cby (t,x[j*m+i]);
           For(k,m)  x[j*m+k] = cminus (x[j*m+k], cby(t1,x[i*m+k]));
           x[j*m+i] = cfactor (t1, -1);
       }
       For(j,m)   x[i*m+j] = cby (x[i*m+j], t);
       x[i*m+i] = t;
   }
   for (i=n-1; i>=0; i--) {
        if (irow[i] == i) continue;
        For(j,n)  {
           t = x[j*m+i];
           x[j*m+i] = x[j*m+irow[i]];
           x[ j*m+irow[i]] = t;
        }
   }
   return (0);
}


void balance(fit_double *mat, int n,int *low, int *hi, fit_double *scale)
{
/* Balance a matrix for calculation of eigenvalues and eigenvectors
*/
    fit_double c,f,g,r,s;
    int i,j,k,l,done;
        /* search for rows isolating an eigenvalue and push them down */
    for (k = n - 1; k >= 0; k--) {
        for (j = k; j >= 0; j--) {
            for (i = 0; i <= k; i++) {
                if (i != j && (fit_double)fabs(mat[pos(j,i,n)]) != 0) break;
            }

            if (i > k) {
                scale[k] = j;

                if (j != k) {
                    for (i = 0; i <= k; i++) {
                       c = mat[pos(i,j,n)];
                       mat[pos(i,j,n)] = mat[pos(i,k,n)];
                       mat[pos(i,k,n)] = c;
                    }

                    for (i = 0; i < n; i++) {
                       c = mat[pos(j,i,n)];
                       mat[pos(j,i,n)] = mat[pos(k,i,n)];
                       mat[pos(k,i,n)] = c;
                    }
                }
                break;
            }
        }
        if (j < 0) break;
    }

    /* search for columns isolating an eigenvalue and push them left */

    for (l = 0; l <= k; l++) {
        for (j = l; j <= k; j++) {
            for (i = l; i <= k; i++) {
                if (i != j && (fit_double)fabs(mat[pos(i,j,n)]) != 0) break;
            }
            if (i > k) {
                scale[l] = j;
                if (j != l) {
                    for (i = 0; i <= k; i++) {
                       c = mat[pos(i,j,n)];
                       mat[pos(i,j,n)] = mat[pos(i,l,n)];
                       mat[pos(i,l,n)] = c;
                    }

                    for (i = l; i < n; i++) {
                       c = mat[pos(j,i,n)];
                       mat[pos(j,i,n)] = mat[pos(l,i,n)];
                       mat[pos(l,i,n)] = c;
                    }
                }

                break;
            }
        }

        if (j > k) break;
    }

    *hi = k;
    *low = l;

    /* balance the submatrix in rows l through k */

    for (i = l; i <= k; i++) {
        scale[i] = 1;
    }

    do {
        for (done = 1,i = l; i <= k; i++) {
            for (c = 0,r = 0,j = l; j <= k; j++) {
                if (j != i) {
                    c += (fit_double)fabs(mat[pos(j,i,n)]);
                    r += (fit_double)fabs(mat[pos(i,j,n)]);
                }
            }

            if (c != 0 && r != 0) {
                g = r / BASE;
                f = 1;
                s = c + r;

                while (c < g) {
                    f *= BASE;
                    c *= BASE * BASE;
                }

                g = r * BASE;

                while (c >= g) {
                    f /= BASE;
                    c /= BASE * BASE;
                }

                if ((c + r) / f < 0.95 * s) {
                    done = 0;
                    g = 1 / f;
                    scale[i] *= f;

                    for (j = l; j < n; j++) {
                        mat[pos(i,j,n)] *= g;
                    }

                    for (j = 0; j <= k; j++) {
                        mat[pos(j,i,n)] *= f;
                    }
                }
            }
        }
    } while (!done);
}


/*
 * Transform back eigenvectors of a balanced matrix
 * into the eigenvectors of the original matrix
 */
void unbalance(int n,fit_double *vr,fit_double *vi, int low, int hi, fit_double *scale)
{
    int i,j,k;
    fit_double tmp;

    for (i = low; i <= hi; i++) {
        for (j = 0; j < n; j++) {
            vr[pos(i,j,n)] *= scale[i];
            vi[pos(i,j,n)] *= scale[i];
        }
    }

    for (i = low - 1; i >= 0; i--) {
        if ((k = (int)scale[i]) != i) {
            for (j = 0; j < n; j++) {
                tmp = vr[pos(i,j,n)];
                vr[pos(i,j,n)] = vr[pos(k,j,n)];
                vr[pos(k,j,n)] = tmp;

                tmp = vi[pos(i,j,n)];
                vi[pos(i,j,n)] = vi[pos(k,j,n)];
                vi[pos(k,j,n)] = tmp;
            }
        }
    }

    for (i = hi + 1; i < n; i++) {
        if ((k = (int)scale[i]) != i) {
            for (j = 0; j < n; j++) {
                tmp = vr[pos(i,j,n)];
                vr[pos(i,j,n)] = vr[pos(k,j,n)];
                vr[pos(k,j,n)] = tmp;

                tmp = vi[pos(i,j,n)];
                vi[pos(i,j,n)] = vi[pos(k,j,n)];
                vi[pos(k,j,n)] = tmp;
            }
        }
    }
}

/*
 * Reduce the submatrix in rows and columns low through hi of real matrix mat to
 * Hessenberg form by elementary similarity transformations
 */
void elemhess(int job,fit_double *mat,int n,int low,int hi, fit_double *vr,
              fit_double *vi, int *work)
{
/* work[n] */
    int i,j,m;
    fit_double x,y;

    for (m = low + 1; m < hi; m++) {
        for (x = 0,i = m,j = m; j <= hi; j++) {
            if ((fit_double)fabs(mat[pos(j,m-1,n)]) > (fit_double)fabs(x)) {
                x = mat[pos(j,m-1,n)];
                i = j;
            }
        }

        if ((work[m] = i) != m) {
            for (j = m - 1; j < n; j++) {
               y = mat[pos(i,j,n)];
               mat[pos(i,j,n)] = mat[pos(m,j,n)];
               mat[pos(m,j,n)] = y;
            }

            for (j = 0; j <= hi; j++) {
               y = mat[pos(j,i,n)];
               mat[pos(j,i,n)] = mat[pos(j,m,n)];
               mat[pos(j,m,n)] = y;
            }
        }

        if (x != 0) {
            for (i = m + 1; i <= hi; i++) {
                if ((y = mat[pos(i,m-1,n)]) != 0) {
                    y = mat[pos(i,m-1,n)] = y / x;

                    for (j = m; j < n; j++) {
                        mat[pos(i,j,n)] -= y * mat[pos(m,j,n)];
                    }

                    for (j = 0; j <= hi; j++) {
                        mat[pos(j,m,n)] += y * mat[pos(j,i,n)];
                    }
                }
            }
        }
    }
    if (job) {
       for (i=0; i<n; i++) {
          for (j=0; j<n; j++) {
             vr[pos(i,j,n)] = 0.0; vi[pos(i,j,n)] = 0.0;
          }
          vr[pos(i,i,n)] = 1.0;
       }

       for (m = hi - 1; m > low; m--) {
          for (i = m + 1; i <= hi; i++) {
             vr[pos(i,m,n)] = mat[pos(i,m-1,n)];
          }

         if ((i = work[m]) != m) {
            for (j = m; j <= hi; j++) {
               vr[pos(m,j,n)] = vr[pos(i,j,n)];
               vr[pos(i,j,n)] = 0.0;
            }
            vr[pos(i,m,n)] = 1.0;
         }
      }
   }
}

/*
 * Calculate eigenvalues and eigenvectors of a real upper Hessenberg matrix
 * Return 1 if converges successfully and 0 otherwise
 */
 
int realeig(int job,fit_double *mat,int n,int low, int hi, fit_double *valr,
      fit_double *vali, fit_double *vr,fit_double *vi)
{
   complex v;
   fit_double p=0,q=0,r=0,s=0,t,w,x,y,z=0,ra,sa,norm,eps;
   int niter,en,i,j,k,l,m;
   fit_double precision  = (fit_double)pow((fit_double)BASE,(fit_double)(1-DIGITS));

   eps = precision;
   for (i=0; i<n; i++) {
      valr[i]=0.0;
      vali[i]=0.0;
   }
      /* store isolated roots and calculate norm */
   for (norm = 0,i = 0; i < n; i++) {
      for (j = MMAX(0,i-1); j < n; j++) {
         norm += (fit_double)fabs(mat[pos(i,j,n)]);
      }
      if (i < low || i > hi) valr[i] = mat[pos(i,i,n)];
   }
   t = 0;
   en = hi;

   while (en >= low) {
      niter = 0;
      for (;;) {

       /* look for single small subdiagonal element */

         for (l = en; l > low; l--) {
            s = (fit_double)fabs(mat[pos(l-1,l-1,n)]) + (fit_double)fabs(mat[pos(l,l,n)]);
            if (s == 0) s = norm;
            if ((fit_double)fabs(mat[pos(l,l-1,n)]) <= eps * s) break;
         }

         /* form shift */

         x = mat[pos(en,en,n)];

         if (l == en) {             /* one root found */
            valr[en] = x + t;
            if (job) mat[pos(en,en,n)] = x + t;
            en--;
            break;
         }

         y = mat[pos(en-1,en-1,n)];
         w = mat[pos(en,en-1,n)] * mat[pos(en-1,en,n)];

         if (l == en - 1) {                /* two roots found */
            p = (y - x) / 2;
            q = p * p + w;
            z = (fit_double)sqrt((fit_double)fabs(q));
            x += t;
            if (job) {
               mat[pos(en,en,n)] = x;
               mat[pos(en-1,en-1,n)] = y + t;
            }
            if (q < 0) {                /* complex pair */
               valr[en-1] = x+p;
               vali[en-1] = z;
               valr[en] = x+p;
               vali[en] = -z;
            }
            else {                      /* real pair */
               z = (p < 0) ? p - z : p + z;
               valr[en-1] = x + z;
               valr[en] = (z == 0) ? x + z : x - w / z;
               if (job) {
                  x = mat[pos(en,en-1,n)];
                  s = (fit_double)fabs(x) + (fit_double)fabs(z);
                  p = x / s;
                  q = z / s;
                  r = (fit_double)sqrt(p*p+q*q);
                  p /= r;
                  q /= r;
                  for (j = en - 1; j < n; j++) {
                     z = mat[pos(en-1,j,n)];
                     mat[pos(en-1,j,n)] = q * z + p *
                     mat[pos(en,j,n)];
                     mat[pos(en,j,n)] = q * mat[pos(en,j,n)] - p*z;
                  }
                  for (i = 0; i <= en; i++) {
                     z = mat[pos(i,en-1,n)];
                     mat[pos(i,en-1,n)] = q * z + p * mat[pos(i,en,n)];
                     mat[pos(i,en,n)] = q * mat[pos(i,en,n)] - p*z;
                  }
                  for (i = low; i <= hi; i++) {
                     z = vr[pos(i,en-1,n)];
                     vr[pos(i,en-1,n)] = q*z + p*vr[pos(i,en,n)];
                     vr[pos(i,en,n)] = q*vr[pos(i,en,n)] - p*z;
                  }
               }
            }
            en -= 2;
            break;
         }
         if (niter == MAXITER) return(-1);
         if (niter != 0 && niter % 10 == 0) {
            t += x;
            for (i = low; i <= en; i++) mat[pos(i,i,n)] -= x;
            s = (fit_double)fabs(mat[pos(en,en-1,n)]) + (fit_double)fabs(mat[pos(en-1,en-2,n)]);
            x = y = 0.75 * s;
            w = -0.4375 * s * s;
         }
         niter++;
           /* look for two consecutive small subdiagonal elements */
         for (m = en - 2; m >= l; m--) {
            z = mat[pos(m,m,n)];
            r = x - z;
            s = y - z;
            p = (r * s - w) / mat[pos(m+1,m,n)] + mat[pos(m,m+1,n)];
            q = mat[pos(m+1,m+1,n)] - z - r - s;
            r = mat[pos(m+2,m+1,n)];
            s = (fit_double)fabs(p) + (fit_double)fabs(q) + (fit_double)fabs(r);
            p /= s;
            q /= s;
            r /= s;
            if (m == l || (fit_double)fabs(mat[pos(m,m-1,n)]) * ((fit_double)fabs(q)+(fit_double)fabs(r)) <=
                eps * ((fit_double)fabs(mat[pos(m-1,m-1,n)]) + (fit_double)fabs(z) +
                (fit_double)fabs(mat[pos(m+1,m+1,n)])) * (fit_double)fabs(p)) break;
         }
         for (i = m + 2; i <= en; i++) mat[pos(i,i-2,n)] = 0;
         for (i = m + 3; i <= en; i++) mat[pos(i,i-3,n)] = 0;
             /* fit_double QR step involving rows l to en and columns m to en */
         for (k = m; k < en; k++) {
            if (k != m) {
               p = mat[pos(k,k-1,n)];
               q = mat[pos(k+1,k-1,n)];
               r = (k == en - 1) ? 0 : mat[pos(k+2,k-1,n)];
               if ((x = (fit_double)fabs(p) + (fit_double)fabs(q) + (fit_double)fabs(r)) == 0) continue;
               p /= x;
               q /= x;
               r /= x;
            }
            s = (fit_double)sqrt(p*p+q*q+r*r);
            if (p < 0) s = -s;
            if (k != m) {
               mat[pos(k,k-1,n)] = -s * x;
            }
            else if (l != m) {
               mat[pos(k,k-1,n)] = -mat[pos(k,k-1,n)];
            }
            p += s;
            x = p / s;
            y = q / s;
            z = r / s;
            q /= p;
            r /= p;
                /* row modification */
            for (j = k; j <= (!job ? en : n-1); j++){
               p = mat[pos(k,j,n)] + q * mat[pos(k+1,j,n)];
               if (k != en - 1) {
                  p += r * mat[pos(k+2,j,n)];
                  mat[pos(k+2,j,n)] -= p * z;
               }
               mat[pos(k+1,j,n)] -= p * y;
               mat[pos(k,j,n)] -= p * x;
            }
            j = MMIN(en,k+3);
              /* column modification */
            for (i = (!job ? l : 0); i <= j; i++) {
               p = x * mat[pos(i,k,n)] + y * mat[pos(i,k+1,n)];
               if (k != en - 1) {
                  p += z * mat[pos(i,k+2,n)];
                  mat[pos(i,k+2,n)] -= p*r;
               }
               mat[pos(i,k+1,n)] -= p*q;
               mat[pos(i,k,n)] -= p;
            }
            if (job) {             /* accumulate transformations */
               for (i = low; i <= hi; i++) {
                  p = x * vr[pos(i,k,n)] + y * vr[pos(i,k+1,n)];
                  if (k != en - 1) {
                     p += z * vr[pos(i,k+2,n)];
                     vr[pos(i,k+2,n)] -= p*r;
                  }
                  vr[pos(i,k+1,n)] -= p*q;
                  vr[pos(i,k,n)] -= p;
               }
            }
         }
      }
   }

   if (!job) return(0);
   if (norm != 0) {
       /* back substitute to find vectors of upper triangular form */
      for (en = n-1; en >= 0; en--) {
         p = valr[en];
         if ((q = vali[en]) < 0) {            /* complex vector */
            m = en - 1;
            if ((fit_double)fabs(mat[pos(en,en-1,n)]) > (fit_double)fabs(mat[pos(en-1,en,n)])) {
               mat[pos(en-1,en-1,n)] = q / mat[pos(en,en-1,n)];
               mat[pos(en-1,en,n)] = (p - mat[pos(en,en,n)]) /
                     mat[pos(en,en-1,n)];
            }
            else {
               v = cdiv(compl(0.0,-mat[pos(en-1,en,n)]),
                    compl(mat[pos(en-1,en-1,n)]-p,q));
               mat[pos(en-1,en-1,n)] = v.re;
               mat[pos(en-1,en,n)] = v.im;
            }
            mat[pos(en,en-1,n)] = 0;
            mat[pos(en,en,n)] = 1;
            for (i = en - 2; i >= 0; i--) {
               w = mat[pos(i,i,n)] - p;
               ra = 0;
               sa = mat[pos(i,en,n)];
               for (j = m; j < en; j++) {
                  ra += mat[pos(i,j,n)] * mat[pos(j,en-1,n)];
                  sa += mat[pos(i,j,n)] * mat[pos(j,en,n)];
               }
               if (vali[i] < 0) {
                  z = w;
                  r = ra;
                  s = sa;
               }
               else {
                  m = i;
                  if (vali[i] == 0) {
                     v = cdiv(compl(-ra,-sa),compl(w,q));
                     mat[pos(i,en-1,n)] = v.re;
                     mat[pos(i,en,n)] = v.im;
                  }
                  else {                      /* solve complex equations */
                     x = mat[pos(i,i+1,n)];
                     y = mat[pos(i+1,i,n)];
                     v.re = (valr[i]- p)*(valr[i]-p) + vali[i]*vali[i] - q*q;
                     v.im = (valr[i] - p)*2*q;
                     if (((fit_double)fabs(v.re) + (fit_double)fabs(v.im)) == 0) {
                        v.re = eps * norm * ((fit_double)fabs(w) +
                                (fit_double)fabs(q) + (fit_double)fabs(x) + (fit_double)fabs(y) + (fit_double)fabs(z));
                     }
                     v = cdiv(compl(x*r-z*ra+q*sa,x*s-z*sa-q*ra),v);
                     mat[pos(i,en-1,n)] = v.re;
                     mat[pos(i,en,n)] = v.im;
                     if ((fit_double)fabs(x) > (fit_double)fabs(z) + (fit_double)fabs(q)) {
                        mat[pos(i+1,en-1,n)] =
                             (-ra - w * mat[pos(i,en-1,n)] +
                             q * mat[pos(i,en,n)]) / x;
                        mat[pos(i+1,en,n)] = (-sa - w * mat[pos(i,en,n)] -
                             q * mat[pos(i,en-1,n)]) / x;
                     }
                     else {
                        v = cdiv(compl(-r-y*mat[pos(i,en-1,n)],
                             -s-y*mat[pos(i,en,n)]),compl(z,q));
                        mat[pos(i+1,en-1,n)] = v.re;
                        mat[pos(i+1,en,n)] = v.im;
                     }
                  }
               }
            }
         }
         else if (q == 0) {                             /* real vector */
            m = en;
            mat[pos(en,en,n)] = 1;
            for (i = en - 1; i >= 0; i--) {
               w = mat[pos(i,i,n)] - p;
               r = mat[pos(i,en,n)];
               for (j = m; j < en; j++) {
                  r += mat[pos(i,j,n)] * mat[pos(j,en,n)];
               }
               if (vali[i] < 0) {
                  z = w;
                  s = r;
               }
               else {
                  m = i;
                  if (vali[i] == 0) {
                     if ((t = w) == 0) t = eps * norm;
                     mat[pos(i,en,n)] = -r / t;
                  }
                  else {            /* solve real equations */
                     x = mat[pos(i,i+1,n)];
                     y = mat[pos(i+1,i,n)];
                     q = (valr[i] - p) * (valr[i] - p) + vali[i]*vali[i];
                     t = (x * s - z * r) / q;
                     mat[pos(i,en,n)] = t;
                     if ((fit_double)fabs(x) <= (fit_double)fabs(z)) {
                        mat[pos(i+1,en,n)] = (-s - y * t) / z;
                     }
                     else {
                        mat[pos(i+1,en,n)] = (-r - w * t) / x;
                     }
                  }
               }
            }
         }
      }
             /* vectors of isolated roots */
      for (i = 0; i < n; i++) {
         if (i < low || i > hi) {
            for (j = i; j < n; j++) {
               vr[pos(i,j,n)] = mat[pos(i,j,n)];
            }
         }
      }
       /* multiply by transformation matrix */

      for (j = n-1; j >= low; j--) {
         m = MMIN(j,hi);
         for (i = low; i <= hi; i++) {
            for (z = 0,k = low; k <= m; k++) {
               z += vr[pos(i,k,n)] * mat[pos(k,j,n)];
            }
            vr[pos(i,j,n)] = z;
         }
      }
   }
    /* rearrange complex eigenvectors */
   for (j = 0; j < n; j++) {
      if (vali[j] != 0) {
         for (i = 0; i < n; i++) {
            vi[pos(i,j,n)] = vr[pos(i,j+1,n)];
            vr[pos(i,j+1,n)] = vr[pos(i,j,n)];
            vi[pos(i,j+1,n)] = -vi[pos(i,j,n)];
         }
         j++;
      }
   }
   return(0);
}
